#pragma once
#include "Iterator.hpp"
#include "array/Array.hpp"
#include "util/Comparator.hpp"
#include "collection/Queue.hpp"
#include "collection/ArrayDeque.hpp"
#include "collection/ArrayList.hpp"
#include "lang/Exception.h"
#include "lang/String.h"
#include "yzrutil.h"
#include <initializer_list>
#include <optional>
#include <memory>

namespace yzrilyzr_collection{
	template<typename E>
	ECLASS(PriorityQueue, public Queue<E>){
	private:
	static constexpr int DEFAULT_INITIAL_CAPACITY=11;
	std::shared_ptr<yzrilyzr_array::Array<E>> queue=nullptr;
	u_index size1=0;
	yzrilyzr_util::Comparator<E> comparator1=nullptr;
	u_index modCount=0;

	void initFromPriorityQueue(PriorityQueue<E>&c){
		this->queue=c.toArray();
		this->size1=c.size();
	}

	void initFromCollection(Collection<E> &c){
		initElementsFromCollection(c);
		heapify();
	}

	void initElementsFromCollection(Collection<E> &c){
		std::shared_ptr<yzrilyzr_array::Array<E>> es=c.toArray();
		u_index len=es->length;
		if(!dynamic_cast<ArrayList<E>*>(&c))
			es=yzrilyzr_array::Arrays::copyOf(*es, len);
		this->queue=es;
		this->size1=len;
	}

	void grow(u_index minCapacity){
		u_index oldCapacity=queue->length;
		u_index newCapacity=yzrilyzr_array::Arrays::newLength(oldCapacity,
															  minCapacity - oldCapacity,
															  oldCapacity < 64?oldCapacity + 2:oldCapacity >> 1
		);
		queue=yzrilyzr_array::Arrays::copyOf(*queue, newCapacity);
	}

	u_index indexOf(const E & o)const{
		std::shared_ptr<yzrilyzr_array::Array<E>> es=queue;
		for(u_index i=0, n=size1; i < n; i++)
			if(o == (*es)[i])
				return i;
		return -1;
	}

	// Heap operations
	void siftUp(u_index k, const E & x){
		if(comparator1 != nullptr)
			siftUpUsingComparator(k, x);
		else
			siftUpNatural(k, x);
	}

	void siftUpNatural(u_index k, const E & x){
		while(k > 0){
			u_index parent=(k - 1) >> 1;
			E & e=(*queue)[parent];
			if(!(x < e))
				break;
			(*queue)[k]=e;
			k=parent;
		}
		(*queue)[k]=x;
	}

	void siftUpUsingComparator(u_index k, const E & x){
		while(k > 0){
			u_index parent=(k - 1) >> 1;
			E & e=(*queue)[parent];
			if(comparator1(x, e) >= 0)
				break;
			(*queue)[k]=e;
			k=parent;
		}
		(*queue)[k]=x;
	}

	void siftDown(u_index k, const E & x){
		if(comparator1 != nullptr)
			siftDownUsingComparator(k, x, size1);
		else
			siftDownNatural(k, x, size1);
	}

	void siftDownNatural(u_index k, const E & x, u_index n){
		u_index half=n >> 1;
		while(k < half){
			u_index child=(k << 1) + 1;
			u_index right=child + 1;

			// 选择较小的子节点（对于最小堆）
			if(right < n && (*queue)[right] < (*queue)[child])
				child=right;

			E & c=(*queue)[child];
			if(!(c < x))  // 如果x不大于c，就停止下沉
				break;

			(*queue)[k]=c;
			k=child;
		}
		(*queue)[k]=x;
	}

	void siftDownUsingComparator(u_index k, const E & x, u_index n){
		u_index half=n >> 1;
		while(k < half){
			u_index child=(k << 1) + 1;
			u_index right=child + 1;
			if(right < n && comparator1((*queue)[child], (*queue)[right]) > 0)
				child=right;
			E & c=(*queue)[child];
			if(comparator1(x, c) <= 0)
				break;
			(*queue)[k]=c;
			k=child;
		}
		(*queue)[k]=x;
	}

	void heapify(){
		u_index n=size1;
		if(n > 0){
			for(u_index i=(n >> 1) - 1; i != std::numeric_limits<u_index>::max(); i--){
				E x=(*queue)[i];
				siftDown(i, x);
			}
		}
	}

	public:
	PriorityQueue() : PriorityQueue(DEFAULT_INITIAL_CAPACITY, nullptr){}

	PriorityQueue(u_index initialCapacity) : PriorityQueue(initialCapacity, nullptr){}

	PriorityQueue(yzrilyzr_util::Comparator<E> comparator) : PriorityQueue(DEFAULT_INITIAL_CAPACITY, comparator){}

	PriorityQueue(u_index initialCapacity, yzrilyzr_util::Comparator<E> comparator){
		if(initialCapacity < 1)
			throw yzrilyzr_lang::IllegalArgumentException();
		this->queue=std::make_shared<yzrilyzr_array::Array<E>>(initialCapacity);
		this->comparator1=comparator;
	}

	PriorityQueue(const Collection<E> &c){
		if(auto cc=dynamic_cast<const PriorityQueue<E>*>(&c)){
			this->comparator1=cc->comparator();
			initFromPriorityQueue(const_cast<PriorityQueue<E>&>(*cc));
		} else{
			this->comparator1=nullptr;
			initFromCollection(const_cast<Collection<E>&>(c));
		}
	}

	PriorityQueue(const std::initializer_list<E>&init) : PriorityQueue(init.size()){
		for(const auto & item : init){
			offer(item);
		}
	}

	bool add(const E & e)override{
		return offer(e);
	}

	bool offer(const E & e)override{
		modCount++;
		u_index i=size1;
		if(i >= queue->length)
			grow(i + 1);
		siftUp(i, e);
		size1=i + 1;
		return true;
	}

	std::optional<E> peek()const noexcept override{
		return (size1 == 0)?std::nullopt:std::optional<E>((*queue)[0]);
	}

	bool remove(const E & o)override{
		u_index i=indexOf(o);
		if(i == -1)
			return false;
		else{
			removeAt(i);
			return true;
		}
	}

	std::optional<E> removeAt(u_index i){
		modCount++;
		u_index s=--size1;
		if(s == i){ // removed last element
			//(*queue)[i]=E();
		} else{
			E moved=(*queue)[s];
			//(*queue)[s]=E();
			siftDown(i, moved);
			if((*queue)[i] == moved){
				siftUp(i, moved);
				if(!((*queue)[i] == moved))
					return std::optional<E>(moved);
			}
		}
		return std::nullopt;
	}

	void removeEq(const E & o){
		auto es=queue;
		for(u_index i=0, n=size1; i < n; i++){
			if(o == (*es)[i]){
				removeAt(i);
				break;
			}
		}
	}

	bool contains(const E & o)const override{
		return indexOf(o) >= 0;
	}

	std::shared_ptr<yzrilyzr_array::Array<E>> toArray()const override{
		return yzrilyzr_array::Arrays::copyOf(*queue, size1);
	}

	u_index size()const override{
		return size1;
	}

	void clear()override{
		modCount++;
		queue=std::make_shared<yzrilyzr_array::Array<E>>(DEFAULT_INITIAL_CAPACITY);
		size1=0;
	}

	std::optional<E> poll()noexcept override{
		if(size1 == 0)
			return std::nullopt;

		modCount++;
		E result=(*queue)[0];
		u_index n=--size1;
		E x=(*queue)[n];
		//(*queue)[n]=E();
		if(n > 0)
			siftDown(0, x);
		return std::optional<E>(result);
	}

	yzrilyzr_util::Comparator<E> comparator() const{
		return comparator1;
	}

	ECLASS(Itr, public Iterator<E>){
	private:
	u_index cursor=0;
	u_index lastRet=-1;
	std::optional<E> lastRetElt=std::nullopt;
	u_index expectedModCount=0;
	const PriorityQueue * clist;
	PriorityQueue * list;
	std::shared_ptr<ArrayDeque<E>> forgetMeNot=nullptr;

	public:
	Itr(const PriorityQueue * clist, PriorityQueue * list) :clist(clist), list(list), expectedModCount(clist->modCount){}

	bool hasNext()const override{
		return cursor < clist->size1 ||
			(forgetMeNot != nullptr && !forgetMeNot->isEmpty());
	}

	E & next()override{
		if(expectedModCount != clist->modCount)
			throw yzrilyzr_lang::ConcurrentModificationException();
		if(cursor < clist->size1)
			return (*clist->queue)[lastRet=cursor++];
		if(forgetMeNot != nullptr){
			lastRet=-1;
			lastRetElt=forgetMeNot->poll();
			if(lastRetElt != std::nullopt)
				return lastRetElt.value();
		}
		throw yzrilyzr_lang::NoSuchElementException();
	}

	void remove()override{
		if(expectedModCount != clist->modCount)
			throw yzrilyzr_lang::ConcurrentModificationException();
		if(lastRet != -1){
			std::optional<E> moved=list->removeAt(lastRet);
			lastRet=-1;
			if(!moved.has_value())
				cursor--;
			else{
				if(forgetMeNot == nullptr)
					forgetMeNot=std::make_shared<ArrayDeque<E>>();
				forgetMeNot->add(moved.value());
			}
		} else if(lastRetElt != std::nullopt){
			list->removeEq(lastRetElt.value());
			lastRetElt=std::nullopt;
		} else{
			throw yzrilyzr_lang::IllegalStateException();
		}
		expectedModCount=clist->modCount;
	}
	};

	std::unique_ptr<Iterator<E>> iterator()override{
		return std::make_unique<Itr>(this, this);
	}

	std::unique_ptr<Iterator<E>> constIterator()const override{
		return std::make_unique<Itr>(this, nullptr);
	}
	};
}